#include <unistd.h>
#include "TestStream.h"

namespace uav
{
namespace comm
{

TestStream::TestStream(const std::string &str)
{
	addInput(str);
}

void TestStream::addInput(const std::string &str)
{
	std::string::const_iterator end = str.end();
	for (std::string::const_iterator i = str.begin(); i != end; i++) {
		input.push(static_cast<uint8_t>(*i));
	}
}

uint8_t TestStream::readByte()
{
	while (input.empty()) usleep(1000);

	uint8_t nextChar = input.front();
	input.pop();
	return nextChar;
}

void TestStream::read(std::vector<uint8_t> &buffer, int numBytes)
{
	buffer.resize(0);
	if (numBytes == 0) {
		while (!input.empty()) {
			buffer.push_back(input.front());
			input.pop();
		}
	} else {
		buffer.reserve(numBytes);
		for (int i = 0; i < numBytes; i++) {
			while (input.empty()) usleep(1000);
			buffer.push_back(input.front());
			input.pop();
		}
	}
}

std::string TestStream::readLine(char terminator)
{
	std::string res;

	char nextChar = 0;
	do {
		while (input.empty()) usleep(1000);
		nextChar = input.front();
		input.pop();
		res += nextChar;
	} while (nextChar != terminator);

	return res;
}

bool TestStream::writeByte(uint8_t dataByte)
{
	output.push_back(dataByte);
	return true;
}

bool TestStream::write(const std::vector<uint8_t> &buffer)
{
	output.insert(output.end(), buffer.begin(), buffer.end());
	return true;
}

bool TestStream::write(const std::string &str)
{
	output.insert(output.end(), str.begin(), str.end());
	return true;
}

}} // end namespace
